////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//
// This file is part of the Microsoft .NET Micro Framework Porting Kit Code Samples and is unsupported.
// Copyright (C) Microsoft Corporation. All rights reserved. Use of this sample source code is subject to
// the terms of the Microsoft license agreement under which you licensed this sample source code.
//
// THIS SAMPLE CODE AND INFORMATION ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED,
// INCLUDING BUT NOT LIMITED TO THE IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
//
//
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

#include <tinyhal.h>

void CPU_FlushCaches()
{
    UINT32 reg = 0;
#ifdef __GNUC__
	asm("MCR p15, 0, %0, c7, c6, 0" :: "r" (reg));
	asm("MCR p15, 0, %0, c7, c10, 4" :: "r" (reg));
	asm("MCR p15, 0, %0, c7, c5, 0" :: "r" (reg));
#else
    __asm
    {
        mcr p15, 0, reg, c7, c6, 0 // invalidate DCache (Write through)

        mcr p15, 0, reg, c7, c10, 4 // Drain write buffer
        mcr p15, 0, reg, c7, c5 , 0 // invalisate Icache
    }
#endif
//  tci_loop:
//  mrc p15, 0, pc, c7, c14, 3 // test clean & invalidate DCache
//  bne tci_loop
}

void CPU_DrainWriteBuffers()
{
    UINT32  reg = 0;

#ifdef __GNUC__
	asm("MCR p15, 0, %0, c7, c10, 4" :: "r" (reg));
#else
    __asm
    {
        mcr     p15, 0, reg, c7, c10, 4 // Drain Write Buffers.
    }
#endif
}

void CPU_InvalidateCaches()
{
    UINT32 reg = 0;

#ifdef __GNUC__
	asm("MOV %0, #0" : "=r" (reg));
	asm("MCR p15, 0, %0, c7, c7, 0" :: "r" (reg));
#else
    __asm
    {
        mov     reg,#0
        mcr     p15, 0, reg, c7, c7 , 0 // Invalidate caches.
    }
#endif
}

void CPU_EnableICache(void )
{
    UINT32 reg = 0;

    CPU_InvalidateCaches();

    __asm
    {
        mrc     p15, 0, reg, c1, c0, 0
        orr     reg, reg, #0x1000             // Enable ICache
        mcr     p15, 0, reg, c1, c0, 0
    }
}

void CPU_EnableCaches()
{
    UINT32 reg;

    CPU_InvalidateCaches();

#ifdef __GNUC__
	asm("MRC p15, 0, %0, c1, c0, 0" : "=r" (reg) : "r" (reg));
	asm("ORR %0, %0, #0x1000" : "=r" (reg) : "r" (reg));
	asm("ORR %0, %0, #0x0004" : "=r" (reg) : "r" (reg));
	asm("MCR p15, 0, %0, c1, c0, 0" :: "r" (reg));
#else
    __asm
    {
        mrc     p15, 0, reg, c1, c0, 0
        orr     reg, reg, #0x1000             // Enable ICache
        orr     reg, reg, #0x0004             // Enable DCache
        mcr     p15, 0, reg, c1, c0, 0
    }
#endif
}

void CPU_DisableCaches()
{
    UINT32 reg;

#ifdef __GNUC__
	asm("MRC p15, 0, %0, c1, c0, 0" : "=r" (reg) : "r" (reg));
	asm("BIC %0, %0, #0x1000" : "=r" (reg) : "r" (reg));
	asm("BIC %0, %0, #0x0004" : "=r" (reg) : "r" (reg));
	asm("MCR p15, 0, %0, c1, c0, 0" :: "r" (reg));
#else
    __asm
    {
        mrc     p15, 0, reg, c1, c0, 0
        bic     reg, reg, #0x1000             // Disable ICache
        bic     reg, reg, #0x0004             // Disable DCache
        mcr     p15, 0, reg, c1, c0, 0
    }
#endif

    CPU_FlushCaches();
}

//--//

template <typename T> void CPU_InvalidateAddress( T* address )
{
    UINT32 reg;

#ifdef __GNUC__
	asm("MOV %0, #0" : "=r" (reg) : "r" (reg));
	asm("MCR p15, 0, %0, c7, c10, 4" :: "r" (reg));
	asm("MCR p15, 0, %0, c7, c5, 1" :: "r" (address));
	asm("MCR p15, 0, %0, c7, c6, 1" :: "r" (address));
#else
	__asm
    {
        mov     reg, #0
        mcr     p15, 0, reg, c7, c10, 4           // Drain Write Buffers.
        mcr     p15, 0, address, c7, c5, 1        // Invalidate DCache.
        mcr     p15, 0, address, c7, c6, 1        // Invalidate DCache.
    }
#endif
}

//--//

#define DCACHE_LINE_IN_WORDS	8
#define DCACHE_LINE_IN_BYTES		(sizeof(long) * DCACHE_LINE_IN_WORDS)
#define DCACHE_MVA_AREA_SIZE	(32*1024*1024)

static inline size_t CPU_GetPid(void)
{
	size_t pid;

	__asm
    {
        MRC     p15, 0, pid, c13, c0, 0           // Get ARM PID
	}

	return pid;
}

static inline void CPU_CleanAndInvalidateDCacheLine(size_t address)
{	
	__asm
    {
        mcr     p15, 0, address, c7, c14, 1
	}
}

static inline void CPU_InvalidateDCacheLine(size_t address)
{
	
	__asm
    {
        mcr     p15, 0, address, c7, c6, 1        // Invalidate DCache.
    }
}

static inline size_t CPU_GetDCacheSize(void)
{
	size_t dsize = 0;

	__asm
    {
		MRC p15, 0, dsize, c0, c0, 1
	}

	//hal_printf("dsize-reg=0x%8.8lx\r\n",dsize);

	dsize = (dsize >> (12+6)) & 0xf;
	dsize = (1 << (dsize-1));
	//hal_printf("dsize=%d\r\n",dsize);

	return dsize;
}

static inline void CPU_WaitForCompletion()
{
	size_t reg;

	__asm
    {
		mrc     p15, 0, reg, c2, c0, 0	// read a register as a nop
		nop
		nop
	}
}

void CPU_MakeDCacheCoherent(size_t address, size_t len)
{
	size_t end;
	size_t dsize = CPU_GetDCacheSize();

	//hal_printf("address=0x%8.8lx len=%d\r\n",address,len);

	if (address && len && dsize) {
		// add extra len due to aligment to len
		len += (address & (DCACHE_LINE_IN_BYTES-1));
		// align address to dcache line start address
		address &= ~(DCACHE_LINE_IN_BYTES-1);
		// align len to dcache line length
		len = (len + (DCACHE_LINE_IN_BYTES-1)) / DCACHE_LINE_IN_BYTES;
		// calc end address
		end = address + (len * DCACHE_LINE_IN_BYTES);
		if (address < DCACHE_MVA_AREA_SIZE) {
			address |= CPU_GetPid();
		}
		//hal_printf("len=%d\r\n",len);
		//hal_printf("end=0x%8.8lx\r\n",address);
		//hal_printf("mva=0x%8.8lx\r\n",address);
		CPU_DrainWriteBuffers();
		CPU_WaitForCompletion();
 		while (address<end) {
			CPU_CleanAndInvalidateDCacheLine(address);
			CPU_DrainWriteBuffers();
			CPU_WaitForCompletion();
			address+=DCACHE_LINE_IN_BYTES;
		}
	}
}

void CPU_DCacheInvalidate(size_t address, size_t len)
{
	size_t end;
	size_t dsize = CPU_GetDCacheSize();

	//hal_printf("address=0x%8.8lx len=%d\r\n",address,len);

	if (address && len && dsize) {
		// add extra len due to aligment to len
		len += (address & (DCACHE_LINE_IN_BYTES-1));
		// align address to dcache line start address
		address &= ~(DCACHE_LINE_IN_BYTES-1);
		// align len to dcache line length
		len = (len + (DCACHE_LINE_IN_BYTES-1)) / DCACHE_LINE_IN_BYTES;
		// calc end address
		end = address + (len * DCACHE_LINE_IN_BYTES);
		if (address < DCACHE_MVA_AREA_SIZE) {
			address |= CPU_GetPid();
		}
		//hal_printf("len=%d\r\n",len);
		//hal_printf("end=0x%8.8lx\r\n",address);
		//hal_printf("mva=0x%8.8lx\r\n",address);
		CPU_DrainWriteBuffers();
		CPU_WaitForCompletion();
		while (address<end) {
			CPU_InvalidateDCacheLine(address);
			CPU_DrainWriteBuffers();
			CPU_WaitForCompletion();
			address+=DCACHE_LINE_IN_BYTES;
		}
	}
}

size_t CPU_GetCachableAddress( size_t address )
{
    //return address & ~AT91SAM::c_UncachableMask;
    return address;
}

//--//

size_t CPU_GetUncachableAddress( size_t address )
{

    // only the flash range is uncachable
    // Note have to use the Constant for Flash memroy base and size,
    // as the MMU will use this function as well,without startingthe MMU,
    // it is not yet ready to access RW area data.

/*
    size_t  temp_address = address & ~AT91SAM::c_UncachableMask;
    if(((UINT32)temp_address >= FLASH_MEMORY_Base ) &&
       ((UINT32)temp_address < (FLASH_MEMORY_Base+FLASH_MEMORY_Size)))
    {
        return address | AT91SAM::c_UncachableMask;
    }
    else
    {
        return address;
    }
*/
    return address;
}
